'''
Created on Feb 23, 2014

@author: yfrank
'''
#fd = "C:\WH\RRT_rotating_poly\MRMPApplication\\"
#exePath = "C:\WH\RRT_rotating_poly\MRMPApplication.exe 0 aa"
import SimulationPeer

import Transport.Enums as Enums

import os , sys , linecache , math
from math import sin
from math import cos
	
class PathFunctions(object):
    '''
    classdocs
    '''
	def __init__(self, dirpath,peer):
		self.outdir = dirpath
		self.inputdir = dirpath
		self.sim_peer = peer
		
	def addRobotWorkspace(self,offset,robotNumber,inOut):
		"""
		addRobotWorkspace:
			add to the workspace a single robot
			inOut - if we want to move the robot to source point inOut = "source"
					else inOut = "target"
			robotNumber - the number of the wanted robot
		"""
		assert(robotNumber>0)
		workspaceFd = open(self.inputdir+"workspace_A.txt","r+")

		wsLines = workspaceFd.readlines()
		workspaceFd.seek(0)

		workspaceFd.write(str(int(wsLines[0].strip())+1)+'\n')
		for i in range(1,len(wsLines)):
			if i == offset+robotNumber:
				workspaceFd.write(self.moveRobot(robotNumber,inOut))
			workspaceFd.write(wsLines[i])
		#if the robot was the last in the file
		if len(wsLines) == offset+robotNumber:
			workspaceFd.write(self.moveRobot(robotNumber,inOut)) 
		workspaceFd.close()
		
	def removeFromWorkspace(self,robotNumber):
		"""
		removeFromWorkspace:
			remove from the workspace a single robot
			robotNumber - the number of the wanted robot
		"""
		assert(robotNumber>0)
		workspaceFd = open(self.inputdir+"workspace_A.txt","r+")
		wsLines = workspaceFd.readlines()

		#clear the contant of the file
		self.eraseContent(workspaceFd)
		#printing the file except for the specific robot
		workspaceFd.write(str(int(wsLines[0].strip())-1)+'\n')
		for i in range(1,len(wsLines)):
			if i != robotNumber:
				workspaceFd.write(wsLines[i])
		workspaceFd.close()
		
	def buildWorkspace(self):
		"""
		buildWorkspace:
			add the robots as obstacle to the workspace
		"""
		robotsFd = open(self.inputdir+"robot_A.txt","r")
		workspaceFd = open(self.inputdir+"workspace_A.txt","r+")

		roboN = int(robotsFd.readline().strip())
		robotsFd.close()
		obsN = int(workspaceFd.readline().strip())+roboN
		#number of obstacle in the workspace

		workspaceFd.seek(0)
		workspaceFd.write(str(obsN)+'\n')

		workspaceFd.seek(0,2)
		for i in range(1,roboN+1):
			workspaceFd.write(self.moveRobot(i,"target"))
		   
		workspaceFd.close()

	def movePolygon(self,coordinates,target):
	#assuming the robot coordinates are around zero
		"""
		movePolygon:
			move the polygon from (0,0) to target 
			coordinates - the original polygon coordinates
			target - the centre of the target coordinates
		"""
		coordinates = coordinates.strip()
		target = target.strip().split()

		nCord = int(coordinates[:coordinates.index(' ')])
		nPoly = str(nCord)
		coordinates = coordinates[coordinates.index(' ')+1:].split()
		
		dx = float(target[0])
		dy = float(target[1])
		ang = (float(target[2])*math.pi)/180

		for i in range(0,nCord):
			x = float(coordinates[2*i])
			y = float(coordinates[2*i+1])

			newX=x*cos(ang)-y*sin(ang)
			newY=x*sin(ang)+y*cos(ang)
			newX+=dx
			newY+=dy
			# can change the turnc if necessary
			nPoly += " %.2f %.2f" % (newX , newY)
		return nPoly
		
	def movePolygonToZero(self,coordinates,source):
		"""
		movePolygonToZero:
			move the polygon from source to (0,0)
			coordinates - the original polygon coordinates
			source - the centre of the source coordinates
		"""
		coordinates = coordinates.strip()
		source = source.strip().split()

		nCord = int(coordinates[:coordinates.index(' ')])
		nPoly = str(nCord)
		coordinates = coordinates[coordinates.index(' ')+1:].split()
		
		dx = float(source[0])
		dy = float(source[1])
		ang = (float(source[2])*math.pi)/180
		
		for i in range(0,nCord):
			x = float(coordinates[2*i])
			y = float(coordinates[2*i+1])

			newX=x-dx
			newY=y-dy
			newX=newX*cos(ang)+newY*sin(ang)
			newY=newY*cos(ang)-newX*sin(ang)

			# can change the turnc if necessary
			nPoly += " %.2f %.2f" % (newX , newY)
		return nPoly
		
	def moveRobot(self,robotNumber,inOut):
		"""
		moveRobot:
			move the robot from (0,0) to source or target
			inOut - if we want to move the robot to source point inOut = "source"
					else inOut = "target"
			robotNumber - the number of the wanted robot
		"""
		assert(robotNumber>0)
		robot = self.getLineFromFile(robotNumber,"robot_A.txt")
		if inOut == "source":
			robot = self.movePolygon(robot,self.getLineFromFile(robotNumber,"source.txt"))
		else:
			robot = self.movePolygon(robot,self.getLineFromFile(robotNumber,"target.txt"))
		return robot+'\n'	

	def changeFileToLines(self,fileName,newFileName):
		"""
		changeFileToLines:
			chnage the file so each polygon will be describe in one line
			fileName - the file we read from
			newFileName - the file we write to
		"""
		assert(fileName != newFileName)
		robotFd = open(self.inputdir+fileName,"r")
		newRobotFd = open(self.inputdir+newFileName,"w")
		newRobotFd.write(robotFd.readline().strip()+'\n')

		while True:
			line = robotFd.readline()
			if not line:
				break
			
			robot= line.strip()
			nVertices = int(robot)

		#for every robot read and write all the coordinates
			for i in range(0,nVertices):
				robot = robot + ' ' + robotFd.readline().strip()

			newRobotFd.write(robot+'\n')
		robotFd.close()
		newRobotFd.close()
			
	def buildQuery(self,robotNumber):
		"""
		buildQuery:
			build the query file from source and target files
			robotNumber - the number of the wanted robot
		"""
		assert(robotNumber>0)
		queryFd = open(self.inputdir+"query.txt","w")
		#TODO use getLineFromFile
		#we flip the source and the target
		source  = linecache.getline(self.inputdir+"source.txt", robotNumber+1).strip()
		target = linecache.getline(self.inputdir+"target.txt", robotNumber+1).strip()

		self.eraseContent(queryFd)

		queryFd.write("\n1\n\n2\n")
		queryFd.write(target+" "+source+"\n")

		queryFd.close()
		linecache.clearcache()
		
	def buildRobot(self,robotNumber):
		"""
		buildRobot:
			build the robot coordinate file
			robotNumber - the number of the wanted robot
		"""
		assert(robotNumber>0)
		robotFd = open(self.inputdir+"robot_S.txt","w")
		#TODO use getLineFromFile
		robot  = linecache.getline(self.inputdir+"robot_A.txt", robotNumber+1)
		linecache.clearcache()	

		eraseContent(robotFd)

		robotFd.write("\n1\n")
		robotFd.write(robot)

		robotFd.close()

	def buildAllRobots(self):
		"""
		buildRobot:
			build the robot coordinate file
			robotNumber - the number of the wanted robot
		"""
		self.changeFileToLines("robot.txt","robot_temp.txt");
		robotFd = open(self.inputdir+"robot_temp.txt","r")
		newRobotsFd = open(self.inputdir+"robot_A.txt","w")

		line = robotFd.readline()
		newRobotsFd.write(line)
		robotNumber=1
						   
		for line in robotFd:
			newRobotsFd.write(self.movePolygonToZero(line,self.getLineFromFile(robotNumber,"source.txt"))+'\n')
			robotNumber+=1
						   
		robotFd.close()
		newRobotsFd.close()
		os.remove(self.inputdir+"robot_temp.txt")

	def getLineFromFile(self,n,fileName):
		"""
		getLineFromFile:
			return: line number n from file
			n - the number of the wanted line
			file - name of the file
		"""
		assert(n>=0)
		line =  linecache.getline(self.inputdir+fileName, n+1)
		linecache.clearcache()
		return line

	def flipRoute(self):
		"""
		flipRoute:
			flip the path from beginning to end 
		"""
		pathFd = open(self.inputdir+"path.txt","r")
		newPathFd = open(self.inputdir+"new_path.txt","w")
		#flip the path
		lines = pathFd.readlines()[::-1]
		robotId = lines.pop().strip()
		newPathFd.write(robotId+'\n')
		robotNumber = int(robotId[robotId.index(' ')+1:])+1 
		targetCoor = self.getLineFromFile(robotNumber,"target.txt").strip()
		for i in range(0,len(lines)):
			if lines[i].startswith('a'):
				#decreasing 180 degrees from the angle
				newPathFd.write("angle %.2f\n" % (float(lines[i][lines[i].index(' ')+1:].strip())*-1))
			else:
				for j in range(i+1,len(lines)):
					foundL=False
					if lines[j].startswith('l'):
						newPathFd.write(lines[j])
						foundL = True
						break
				if not foundL:
						newPathFd.write("line " +targetCoor[:targetCoor.rindex(' ')]+ '\n')
					
		#add the target x y
		#need to recive folder of target
		
		pathFd.close()
		newPathFd.close()

	def eraseContent(self,fd):
		"""
		eraseContent:
			erase the file content
			fd - file descriptor
		"""
		fd.seek(0)
		fd.truncate()
		fd.seek(0)

	def solveS(self,exePath):
		os.chdir(self.inputdir)

		nRobot = int(self.getLineFromFile(0,"robot.txt").strip())

		nObstaculos = int(self.getLineFromFile(0,"workspace.txt").strip())

		robList = [i+1 for i in range(0,nRobot)]
		currRobot = 0
		pathFound = 1

		self.buildAllRobots()
		self.changeFileToLines("workspace.txt","workspace_A.txt")

		self.buildWorkspace()

		while robList:
			self.buildQuery(robList[currRobot])
			self.buildRobot(robList[currRobot])
			self.removeFromWorkspace(nObstaculos+robList[currRobot])
			result = os.system(exePath)
			if result:
			# the path title is the number of path found
				self.flipRoute()
				self.addRobotWorkspace(nObstaculos,robList[currRobot],"source")
				del(robList[currRobot])
				nRobot -= 1
			else:
				self.addRobotWorkspace(nObstaculos,robList[currRobot],"target")
				currRobot = (currRobot+1) % nRobot
		#os.remove(path)


    def notify_success(self):
        self.sim_peer.handle_event(Enums.PeerEvent.SUCC)


    def notify_failure(self):
        self.sim_peer.handle_event(Enums.PeerEvent.FAIL)

    def do_calculate_path(self): #TODO replace with...
		self.solveS(exePath) # TODO need to add the exePath
        self.sim_peer.handle_event(Enums.PeerEvent.SUCC)
